function Xres = UWBLocIters(Obs,para)

% BsNum = para.BsNum;
% BsLoc = para.BsLoc;
% 
% x=BsLoc(:,1)';
% y=BsLoc(:,2)';
% z=BsLoc(:,3)';
% 
% data=Obs
% num=length(data);
% distance=zeros(1,4,num);
% for j=1:4
%     for h=1:num
%         distance(1,j,h)=data(h,j);
%     end
% end
% 
% A = zeros(3,3,num);
% b = zeros(3,1,num);
% c = zeros(3,1,num);
% 
% for k=1:num
%     A(:,:,k) = 2*([x(2)-x(1),y(2)-y(1),z(2)-z(1);
%                    x(3)-x(1),y(3)-y(1),z(3)-z(1);
%                    x(4)-x(1),y(4)-y(1),z(4)-z(1)]);
%           
%     b(:,:,k) = [distance(1,1,k)^2-distance(1,2,k)^2+x(2)^2-x(1)^2+y(2)^2-y(1)^2+z(2)^2-z(1)^2;
%                 distance(1,1,k)^2-distance(1,3,k)^2+x(3)^2-x(1)^2+y(3)^2-y(1)^2+z(3)^2-z(1)^2;
%                 distance(1,1,k)^2-distance(1,4,k)^2+x(4)^2-x(1)^2+y(4)^2-y(1)^2+z(4)^2-z(1)^2];
% end
% A_SUM = zeros(3*num,3);
% b_SUM = zeros(3*num,1);
% for k=1:num
%     A_SUM(k*3-2:k*3,1:3)=A(:,:,1);
%     b_SUM(k*3-2:k*3,1)=b(:,:,1);
% end
% sum = inv(A_SUM'*A_SUM)*(A_SUM'*b_SUM);
% Xres=sum;
Thr = para.Thr; 
DataLen = para.DataLen; 

BsNum = para.BsNum;
BsLoc = para.BsLoc;

dRef = para.dRef;

Xres = zeros(DataLen,3); 


Xinit = para.xinit;
MaxIters = para.MaxIters;

for ii=1:DataLen
    
    X = Xinit;
    ObsX = Obs(ii,:);
    for it=1:MaxIters
        ObsE = UWBToaObs(BsLoc,BsNum,X);    
        Err = ObsX-ObsE;
        J = Jcal(BsNum,X,BsLoc);
        w = wcal(ObsX,dRef);
        dX = (J'*w*J)^(-1)*J'*w*Err';
        
        X = X+dX';
        if (sum(abs(Err))<Thr)
            break;
        end
    end
    Xres(ii,:) = X;
    
end
 
end

function w = wcal(Obs,dRef)
w = 1+0.01*(abs(Obs-dRef)+1);
w(Obs>dRef) = 1./(abs(Obs(Obs>dRef)-dRef)+1);
w = diag(w);
end

function J = Jcal(bsNum,loc,BsLoc)
J = zeros(bsNum,3);
dd = 0.01;
Res0 = UWBToaObs(BsLoc,bsNum,loc);
Resx1 = UWBToaObs(BsLoc,bsNum,loc+[dd 0 0]);
Resy1 = UWBToaObs(BsLoc,bsNum,loc+[0 dd 0]);
Resz1 = UWBToaObs(BsLoc,bsNum,loc+[0 0 dd]);

J(:,1)=(Resx1-Res0)'/dd;
J(:,2)=(Resy1-Res0)'/dd;
J(:,3)=(Resz1-Res0)'/dd;

end